#include "wild_mot.h"
#include "ck_private_macro.h"
#include "wild_mdl.h"


void MatrixMultiply(ckMat& result, const ckMat& s1, const ckMat& s2)
{
	register r32 s1Data_[16];
	register r32 s2Data_[16];

	s1.toR32x16( s1Data_ );
	s2.toR32x16( s2Data_ );

	register r32 result_[16];
	
	ckMat::mulR32x16( result_, s1Data_, s2Data_ );

	result = ckMat::fromR32x16( result_ );

	
	/*
	r32* s1Data_ = NULL;
	r32* s2Data_ = NULL;

	ckNewArray( s1Data_, r32, 16 );
	ckNewArray( s2Data_, r32, 16 );

	s1.toR32x16( s1Data_ );
	s2.toR32x16( s2Data_ );

	

	r32* result_ = NULL;
	ckNewArray( result_, r32, 16 );
	//ckMemMgr::memset( result_, 0, 16 );

	ckMat::mulR32x16( result_, s1Data_, s2Data_ );

	result = ckMat::fromR32x16( result_ );

	ckDeleteArray( s1Data_, r32 );
	ckDeleteArray( s2Data_, r32 );
	ckDeleteArray( result_, r32 );
	//*/
	
}

WildMot::WildMot()
{
    m_interp_info = NULL;

//	m_startPose = ckMat::ZERO;
	m_meshLocal = ckMat::ZERO;
}


WildMot::~WildMot()
{
    uninit();
}


void WildMot::init(/*wildMdl* mdl,*/ ckID mot_data_id)
{
    uninit();

    if (mot_data_id == ckID::ZERO)
    {
        ckThrow(ExceptionInvalidArgument);
    }

    ckRes res;

    ckTry
    {
        res = ckResMgr::getResource(mot_data_id);
    }
    ckCatch(ckResMgr::ExceptionNotFound)
    {
        ckThrow(ExceptionInvalidArgument);
    }

    m_mot_data.initAsReader(res.getData<void>(), res.getDataSize());

    ckNewArray(m_interp_info, InterpInfo, m_mot_data.getNodeNum());

    m_play_mode = PLAY_NORMAL;
    m_mot_index = 0;
    m_interp_frame = 0;
    m_play_speed = 1.0f;
    m_playing_frame = -1.0f;
    m_interp_speed = 0.0f;
    m_interp_ratio = 0.0f;
}


u16 WildMot::getMotionIndex() const
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    return m_mot_index;
}


WildMot::PlayMode WildMot::getPlayMode() const
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    return m_play_mode.getType();
}


r32 WildMot::getPlaySpeed() const
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    return m_play_speed;
}


u16 WildMot::getInterpFrame() const
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    return m_interp_frame;
}


bool WildMot::isPlaying() const
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    return (m_playing_frame >= 0.0f);
}


void WildMot::play(ckMat pose, u16 mot_index, PlayMode play_mode, r32 play_speed, u16 interp_frame)
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    if ( mot_index >= m_mot_data.getMotionNum())
    {
        ckThrow(ExceptionInvalidArgument);
    }

    m_play_mode = play_mode;
    m_play_speed = play_speed;
    m_mot_index = mot_index;
    m_interp_frame = interp_frame;
    m_playing_frame = 0.0f;

    if (interp_frame > 0)
    {
        m_interp_speed = 1.0f / interp_frame;
        m_interp_ratio = 1.0f;

        //const WildMdlData* mdl_data = mdl->getModelData();
        //u16 node_num = ckMath::min(m_mot_data.getNodeNum(), mdl_data->getNodeNum());

        //for (s32 i = 0; i < node_num; i++)
        //{
            InterpInfo* interp_info = &m_interp_info[0];
            //ckMat local = mdl->getNodeDraw(i)->local().toLocalOf(mdl_data->getNodeLocal(i));


			//MatrixMultiply( pose, pose, m_meshLocal	);
			/*pose.trans += m_meshLocal.trans;
			pose.x_axis += m_meshLocal.x_axis;
			pose.y_axis += m_meshLocal.y_axis;
			pose.z_axis += m_meshLocal.z_axis;*/

            interp_info->quat = ckQuat::fromMat(pose);
            interp_info->trans = pose.trans;
        //}
    }
    else
    {
        m_interp_speed = 0.0f;
        m_interp_ratio = 0.0f;
    }
}


void WildMot::stop()
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    m_playing_frame = -1.0f;
}


r32 WildMot::getNextUpdateFrame() const
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    return (m_playing_frame < 0.0f) ? 0.0f : m_playing_frame;
}


ckMat WildMot::update( r32* nowPlayingFrame )
{
    if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    

    if (!isPlaying())
    {
		return ckMat::UNIT;
    }

    /*
        find current key_frame_index
    */
    u32 key_frame_num = m_mot_data.getMotionKeyFrameNum(m_mot_index);
    u32 first_key_frame_index = m_mot_data.getMotionKeyFrameIndex(m_mot_index);
    u32 last_key_frame_index = first_key_frame_index + key_frame_num - 1;

    u32 key_frame_index;

    for (key_frame_index = last_key_frame_index; key_frame_index > first_key_frame_index; key_frame_index--)
    {
        if (m_playing_frame >= m_mot_data.getKeyFramePlayFrame(key_frame_index))
        {
            break;
        }
    }

	//test
	//key_frame_index = 0;
	//InnerMdlTrace( key_frame_index, "key_frame_index" );

    /*
        calculate blend ratio
    */
    u32 cur_key_frame_play_frame = m_mot_data.getKeyFramePlayFrame(key_frame_index);

    r32 blend_ratio = (key_frame_index >= last_key_frame_index) ? 0.0f : //
        (m_playing_frame - cur_key_frame_play_frame) / (m_mot_data.getKeyFramePlayFrame(key_frame_index + 1) - cur_key_frame_play_frame);

    bool is_blend = (blend_ratio > 0.0f);

    /*
        update interpolate ratio
    */
    if (m_interp_ratio > 0.0f)
    {
        m_interp_ratio -= m_interp_speed * m_play_speed;

        if (m_interp_ratio < 0.0f)
        {
            m_interp_ratio = 0.0f;
        }
    }

    bool is_interp = (m_interp_ratio > 0.0f);

    /*
        update model's local matrix
    */
    //const WildMdlData* mdl_data = mdl->getModelData();
    //u16 node_num = ckMath::min(m_mot_data.getNodeNum(), mdl_data->getNodeNum());

	//u16 for_node_index = m_mot_data.getForNodeIndex();

	ckMat result_ = ckMat::UNIT;
    //for (s32 i = 0; i < node_num; i++)
	//if(for_node_index<node_num)
    {
		//s32 i = for_node_index;

		

		s32 i = 0;


        ckQuat quat = m_mot_data.getNodeRotate(i, key_frame_index);
        ckVec trans = m_mot_data.getNodeTrans(i, key_frame_index);

        if (is_blend)
        {
            ckQuat quat2 = m_mot_data.getNodeRotate(i, key_frame_index + 1); // to avoid runtime error on iPhone
            ckVec trans2 = m_mot_data.getNodeTrans(i, key_frame_index + 1); // to avoid runtime error on iPhone

            quat = quat.slerp(quat2, blend_ratio);
            trans = trans.interp(trans2, blend_ratio);
        }

        if (is_interp)
        {
            InterpInfo* interp_info = &m_interp_info[i];

            quat = quat.slerp(interp_info->quat, m_interp_ratio);
            trans = trans.interp(interp_info->trans, m_interp_ratio);
        }

		
		
		

		//	update local matrix
		//mdl->getNodeDraw(i)->local() = ( quat.toMat(trans) ).toGlobalFrom(mdl_data->getNodeLocal(i));

		//	update and combine model data
		//	...

		result_ = ( quat.toMat(trans) );

		//MatrixMultiply( result_, m_meshLocal, result_ );
		/*result_.trans += m_meshLocal.trans;
		result_.x_axis += m_meshLocal.x_axis;
		result_.y_axis += m_meshLocal.y_axis;
		result_.z_axis += m_meshLocal.z_axis;*/
		
    }

    /*
        update current frame
    */
    r32 frame_num = static_cast<r32>(m_mot_data.getMotionFrameNum(m_mot_index));
    bool is_frame_remaining = (m_playing_frame < frame_num);

    m_playing_frame += m_play_speed;
	//InnerMdlTrace( m_playing_frame, "playing_frame_index" );

    if (m_playing_frame > frame_num)
    {
        if (m_play_mode == PLAY_LOOP)
        {
            m_playing_frame -= frame_num;
        }
        else if (m_play_mode == PLAY_KEEP || is_frame_remaining)
        {
            m_playing_frame = frame_num;
        }
        else
        {
            m_playing_frame = -1.0f;
        }
    }

	//	test
	//m_playing_frame = 1;

	*nowPlayingFrame = m_playing_frame;
	

	return result_;
}


ckMat WildMot::updateByRootFrame( r32 nowPlayingFrame )
{
	if (!m_interp_info)
    {
        ckThrow(ExceptionNotInitialized);
    }

    

    if (!isPlaying())
    {
		return ckMat::UNIT;
    }

	/*
        update current frame
    */
    m_playing_frame = nowPlayingFrame;


    /*
        find current key_frame_index
    */
    u32 key_frame_num = m_mot_data.getMotionKeyFrameNum(m_mot_index);
    u32 first_key_frame_index = m_mot_data.getMotionKeyFrameIndex(m_mot_index);
    u32 last_key_frame_index = first_key_frame_index + key_frame_num - 1;

    u32 key_frame_index;

    for (key_frame_index = last_key_frame_index; key_frame_index > first_key_frame_index; key_frame_index--)
    {
        if (m_playing_frame >= m_mot_data.getKeyFramePlayFrame(key_frame_index))
        {
            break;
        }
    }

	//test
	//key_frame_index = 0;
	//InnerMdlTrace( key_frame_index, "key_frame_index" );

    /*
        calculate blend ratio
    */
    u32 cur_key_frame_play_frame = m_mot_data.getKeyFramePlayFrame(key_frame_index);

    r32 blend_ratio = (key_frame_index >= last_key_frame_index) ? 0.0f : //
        (m_playing_frame - cur_key_frame_play_frame) / (m_mot_data.getKeyFramePlayFrame(key_frame_index + 1) - cur_key_frame_play_frame);

    bool is_blend = (blend_ratio > 0.0f);

    /*
        update interpolate ratio
    */
    if (m_interp_ratio > 0.0f)
    {
        m_interp_ratio -= m_interp_speed * m_play_speed;

        if (m_interp_ratio < 0.0f)
        {
            m_interp_ratio = 0.0f;
        }
    }

    bool is_interp = (m_interp_ratio > 0.0f);

    /*
        update model's local matrix
    */
    

	ckMat result_ = ckMat::UNIT;
    
    {
		

		

		s32 i = 0;


        ckQuat quat = m_mot_data.getNodeRotate(i, key_frame_index);
        ckVec trans = m_mot_data.getNodeTrans(i, key_frame_index);

        if (is_blend)
        {
            ckQuat quat2 = m_mot_data.getNodeRotate(i, key_frame_index + 1); // to avoid runtime error on iPhone
            ckVec trans2 = m_mot_data.getNodeTrans(i, key_frame_index + 1); // to avoid runtime error on iPhone
			

            quat = quat.slerp(quat2, blend_ratio);
            trans = trans.interp(trans2, blend_ratio);
        }

        if (is_interp)
        {
            InterpInfo* interp_info = &m_interp_info[i];

            quat = quat.slerp(interp_info->quat, m_interp_ratio);
            trans = trans.interp(interp_info->trans, m_interp_ratio);
        }

		
		
		

		

		result_ = ( quat.toMat(trans) );

		//MatrixMultiply( result_, m_meshLocal, result_ );
		/*result_.trans += m_meshLocal.trans;
		result_.x_axis += m_meshLocal.x_axis;
		result_.y_axis += m_meshLocal.y_axis;
		result_.z_axis += m_meshLocal.z_axis;*/
    }

    

	return result_;

	
}


//void WildMot::changeModelByWeight(SkinInfo* skin, WildMdlData* model, const ckMat& parentMat)
//{
//	ckMat offsetM_ = ckMat::ZERO;
//		
//	u16 total_ = model->getVertNum();
//
//	ckVec t_ = ckVec::ZERO;
//	ckMat localMat_ = ckMat::UNIT;
//
//	for( u16 i=0;i<skin->totalVertexNum;i++ )
//	{
//		t_ = ckVec::ZERO;
//		localMat_ = ckMat::UNIT;
//
//		if(skin->vertexIdx[i]>=0&&skin->vertexIdx[i]<total_)
//		{
//			const ckVec& v_ = model->getVertPos( skin->vertexIdx[i] );
//
//			offsetM_ = ckMat::UNIT;
//			if(parentMat.isUnit())
//			{
//				offsetM_ = skin->offsetMat;
//			}
//			else
//			{
//				
//				MatrixMultiply( offsetM_, parentMat, skin->offsetMat );
//			}
//
//
//			offsetM_.trans *= skin->vertexWeight[i];
//			offsetM_.x_axis *= skin->vertexWeight[i];
//			offsetM_.y_axis *= skin->vertexWeight[i];
//			offsetM_.z_axis *= skin->vertexWeight[i];
//
//			t_ = v_;
//			t_.toGlobalFrom( offsetM_ );
//			
//
//			model->setVertPos( i, t_ );
//		}
//	}
//}


CK_DEFINE_COPY_CONSTRUCTOR(WildMot)


CK_DEFINE_OPERATOR_EQUAL(WildMot)


void WildMot::uninit()
{
    if (m_interp_info)
    {
        ckDeleteArray(m_interp_info, InterpInfo);

        m_interp_info = NULL;
    }
}
